/*
* 时间：2021.07.22
* 代码来源：公众号【从0到1的点云】
* 功能：实现点云对平面的投影
*/
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/project_inliers.h>
#include<stdio.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
#include <vector>
#include <ctime>

void random_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud, int number)
{
  srand((unsigned int)time(NULL));
  (*cloud).height = 1;//random cloud flag
  (*cloud).width = number;
  (*cloud).points.resize(((*cloud).height)*((*cloud).width));
  for (size_t i = 0; i < (*cloud).points.size(); i++)
  {
    (*cloud).points[i].x = 64.0f*rand() / (RAND_MAX + 1.0F);
    (*cloud).points[i].y = 64.0f*rand() / (RAND_MAX + 1.0F);
    (*cloud).points[i].z = 64.0f*rand() / (RAND_MAX + 1.0F);
  }
};
int main()
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZ>);

  // Fill in the cloud data
  random_cloud(cloud, 500);

  std::cerr << "Cloud before projection: " << std::endl;
  //for (size_t i = 0; i < cloud->points.size(); ++i)
  //  std::cerr << "    " << cloud->points[i].x << " "
  //  << cloud->points[i].y << " "
  //  << cloud->points[i].z << std::endl;

  // Create a set of planar coefficients with X=Y=0,Z=1
  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
  coefficients->values.resize(4);
  coefficients->values[0] = 1;
  coefficients->values[1] = 0;
  coefficients->values[2] = 0;
  coefficients->values[3] = 0;

  // Create the filtering object
  pcl::ProjectInliers<pcl::PointXYZ> proj;
  proj.setModelType(pcl::SACMODEL_PLANE);
  proj.setInputCloud(cloud);
  proj.setModelCoefficients(coefficients);
  proj.filter(*cloud_projected);

  std::cerr << "Cloud after projection: " << std::endl;
  //for (size_t i = 0; i < cloud_projected->points.size(); ++i)
  //  std::cerr << "    " << cloud_projected->points[i].x << " "
  //  << cloud_projected->points[i].y << " "
  //  << cloud_projected->points[i].z << std::endl;
  std::cout << "投影前点的个数" << cloud->size() << std::endl;
  std::cout << "投影后点的个数" << cloud_projected->size() << std::endl;

  //可视化
  pcl::visualization::PCLVisualizer Viewer;
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>C_B_P(cloud, 255, 0, 0); //C_B_P=cloud before projection首字母简写
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>C_A_P(cloud_projected, 0, 255, 0); //C_A_P=cloud after projection首字母简写                                                                                                                                                //C_A_P=cloud after projection首字母简写
  Viewer.addPointCloud(cloud, C_B_P, "cloud before projection");
  Viewer.addPointCloud(cloud_projected, C_A_P, "cloud after projection");
  Viewer.spin();
  getchar();
}